import time

from InitialPositionOfGuide import *
from GTAPI import GtMainAuto, GtAlsAuto
from RobotArmAPI import arm

_mobile_phone_warehouse = {
    6: Clamp_position_one,
    5: Clamp_position_two,
    4: Clamp_position_three,
    3: Clamp_position_four,
    2: Clamp_position_five,
    1: Clamp_position_six,
}

_mobile_phone_warehouse_location = {
    6: Iphone_position_one,
    5: Iphone_position_two,
    4: Iphone_position_three,
    3: Iphone_position_four,
    2: Iphone_position_five,
    1: Iphone_position_six,
}


# Main moving guide
def task_flow_main(main_guide_rail_position):
    GtMainAuto.DominantRailMovement(0, 1200)
    if main_guide_rail_position.card_id == 0:
        GtMainAuto.ChartGoHome()
    if main_guide_rail_position.amo:
        GtMainAuto.SetWorkPosition_main(3)
    else:
        GtMainAuto.SetStopPosition_main(3)
    if main_guide_rail_position.curtain:
        GtMainAuto.SetStopPosition_main(5)
        GtMainAuto.SetStopPosition_main(4)
        GtMainAuto.SetWorkPosition_main(6)
    else:
        GtMainAuto.SetStopPosition_main(7)
        GtMainAuto.SetStopPosition_main(5)
        GtMainAuto.SetStopPosition_main(4)
        GtMainAuto.SetStopPosition_main(6)

    if main_guide_rail_position.kino:
        GtMainAuto.SetStopPosition_main(5)
        GtMainAuto.SetWorkPosition_main(4)
    else:

        GtMainAuto.SetStopPosition_main(7)
        time.sleep(0.1)
        GtMainAuto.SetStopPosition_main(5)
        time.sleep(0.1)
        GtMainAuto.SetStopPosition_main(4)

    if main_guide_rail_position.timer_right_left:
        GtMainAuto.SetWorkPosition_main(4)
    else:
        GtMainAuto.SetStopPosition_main(5)

    if main_guide_rail_position.timer_front_back:
        GtMainAuto.Gotimer_2()
        time.sleep(0.1)
        GtMainAuto.SetWorkPosition_main(7)

    else:
        GtMainAuto.SetStopPosition_main(7)
    if main_guide_rail_position.card_id != 0:
        if main_guide_rail_position.card_id in [1, 2, 4, 6, 8, 9, 10]:
            GtMainAuto.ChartSwitching(main_guide_rail_position.card_id)
        else:
            GtMainAuto.ChartSwitching(6)
    else:
        GtMainAuto.ChartGoHome()
    GtMainAuto.DominantRailMovement(2, main_guide_rail_position.right_left)
    GtMainAuto.DominantRailMovement(1, main_guide_rail_position.up_down)
    GtMainAuto.DominantRailMovement(0, main_guide_rail_position.front_back)
    return 'Move complete'


# The robot arm picks up the phone
def task_flow_arm(location):
    arm.Set_joint_move(_mobile_phone_warehouse[int(location)], 0.5, 0.5)
    arm.Set_tool_move(_mobile_phone_warehouse_location[int(location)], 0.1, 0.1, 1)
    res = arm.Set_standard_digital_out_release_arm()
    if res == 'Release success':
        arm.Set_tool_move(_mobile_phone_warehouse_location[int(location)], 0.05, 0.05, 0)
        res = arm.Set_standard_digital_out_clamp_arm(location)

        if res == 'Successful clamp':
            arm.Set_tool_move(_mobile_phone_warehouse_location[int(location)], 0.1, 0.1, 1)
            arm.Set_joint_move(Height_Position, 0.5, 0.5)
            time.sleep(1)
            arm.Set_joint_move(Alignment_Position_on, 0.5, 0.5)
            time.sleep(1)
            arm.Set_joint_move(Alignment_Position, 0.5, 0.5)
    return 'Mechanical arm movement completed'


# Put the arm back on the phone
def task_release_arm(location):
    arm.Set_joint_move(Height_Position, 0.5, 0.5)
    time.sleep(2)
    arm.Set_joint_move(_mobile_phone_warehouse[int(location[0])], 0.5, 0.5)
    arm.Set_tool_move(_mobile_phone_warehouse_location[int(location[0])], 0.1, 0.1, 1)
    arm.Set_tool_move(_mobile_phone_warehouse_location[int(location[0])], 0.05, 0.05, 0)
    res = arm.Set_standard_digital_out_release_arm()
    if int(location[0]) == 1 or int(location[0]) == 2:
        arm.Set_standard_digital_out(4, 1)  # 松开手机定位夹
    if int(location[0]) == 4 or int(location[0]) == 3:
        arm.Set_standard_digital_out(5, 1)  # 松开手机定位夹
    if int(location[0]) == 6 or int(location[0]) == 5:
        arm.Set_standard_digital_out(6, 1)  # 松开手机定位夹
    print(res)
    if res == 'Release success':
        arm.Set_tool_move(_mobile_phone_warehouse_location[int(location[0])], 0.1, 0.1, 1)
    return 'Phone back successfully'


# Light axis shift
def task_flow_light(light_guide_rail_position):
    print()
    GtAlsAuto.MovingLightGuide(0, light_guide_rail_position.left_front_back)
    GtAlsAuto.MovingLightGuide(1, light_guide_rail_position.left_right_left)
    GtAlsAuto.MovingLightGuide(4, light_guide_rail_position.left_rotation)
    GtAlsAuto.MovingLightGuide(2, light_guide_rail_position.right_front_back)
    GtAlsAuto.MovingLightGuide(3, light_guide_rail_position.right_right_left)
    GtAlsAuto.MovingLightGuide(5, light_guide_rail_position.right_rotation)
    return 'Move complete'


# Determine whether the mobile phone is clipped to the robot arm
def task_flow_arm_is_iphone():
    is_iphone = []
    res = arm.Get_configurable_input_mes()
    for index, i in enumerate(res):
        if i == 0:
            is_iphone.append(index + 1)
    return is_iphone


def _main(phone, main_guide_rail_position, light_guide_rail_position):
    task_flow_light(light_guide_rail_position)

    # # # Dominant rail movement
    task_flow_main(main_guide_rail_position)
    # Determine if the robot arm is holding the phone

    res = task_flow_arm_is_iphone()
    if res and int(res[0]) != int(phone):
        arm.adjust_rotation_Z(0.3)
        task_release_arm(res)

    # Grab the phone and move it to the designated location
    # if int(res[0]) != int(phone):
    if not res or int(res[0]) != int(phone):
        task_flow_arm(phone)
    else:
        arm.Set_joint_move(Alignment_Position_on, 0.5, 0.5)
        time.sleep(0.5)
        arm.Set_joint_move(Alignment_Position, 0.5, 0.5)
    # Light axis shift
    # task_flow_light(light_guide_rail_position)
    return 'Move complete'


0
if __name__ == '__main__':
    GtMainAuto.DominantRailMovement(0, 900-146.47823297*1.5)
    # GtMainAuto.DominantRailMovement(0, main_guide_rail_position.front_back - RES * MTF_JL[0]['ratio'])
    # task_flow_light(Guide_Light)
    #     while True:
    #         _main(1)
    #         _main(5)
    #         _main(4)
    # _main(4)
    #     _main(1,1,1)
    #     _main(2,1,1)
    # arm.Set_joint_move(Alignment_Position, 0.5, 0.5)

# arm.Set_standard_digital _out(4, 1)
# arm.Set_standard_digital_out(6,0)
# task_flow_light(Guide_Light)
